{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#  目标巡线 - 演示示例"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "在此示例中，我们将使用经过训练的模型来使jetBot平稳地在轨道上移动。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 加载已训练好的模型"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "我们将假定您已经按照“ train_model.ipynb”笔记本中的说明将“ best_steering_model_xy.pth”下载到工作站。 现在，您应该将模型文件上传到该笔记本目录中的JetBot。 完成后，在此笔记本的目录中应该有一个名为“ best_steering_model_xy.pth”的文件。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "> 在调用下一个单元格之前，请确保文件已完全上传"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "执行以下代码以初始化PyTorch模型。 从训练示例中看起来应该非常熟悉。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import torchvision\n",
    "import torch\n",
    "\n",
    "model = torchvision.models.resnet18(pretrained=False)\n",
    "model.fc = torch.nn.Linear(512, 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "接下来，从您上传的“ best_steering_model_xy.pth”文件中加载训练后的权重。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "model.load_state_dict(torch.load('best_steering_model_xy.pth'))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "当前，模型权重位于CPU内存上，执行以下代码以传输到GPU设备。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "device = torch.device('cuda')\n",
    "model = model.to(device)\n",
    "model = model.eval().half()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 创建预处理函数"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "现在，我们已经加载了模型，但是有一个小问题。 我们训练模型的格式与相机的格式不完全匹配。 为此，我们需要进行一些预处理。 这涉及以下步骤：\n",
    "\n",
    "1.从HWC布局转换为CHW布局\n",
    "2.使用与训练期间相同的参数进行归一化（我们的相机提供[0，255]范围内的值，并训练[0，1]范围内的加载图像，因此我们需要按255.0缩放\n",
    "3.将数据从CPU内存传输到GPU内存\n",
    "4.添加批次尺寸"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import torchvision.transforms as transforms\n",
    "import torch.nn.functional as F\n",
    "import cv2\n",
    "import PIL.Image\n",
    "import numpy as np\n",
    "\n",
    "mean = torch.Tensor([0.485, 0.456, 0.406]).cuda().half()\n",
    "std = torch.Tensor([0.229, 0.224, 0.225]).cuda().half()\n",
    "\n",
    "def preprocess(image):\n",
    "    image = PIL.Image.fromarray(image)\n",
    "    image = transforms.functional.to_tensor(image).to(device).half()\n",
    "    image.sub_(mean[:, None, None]).div_(std[:, None, None])\n",
    "    return image[None, ...]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "太棒了！ 现在，我们定义了预处理功能，该功能可以将图像从相机格式转换为神经网络输入格式。\n",
    "\n",
    "现在，让我们开始展示相机。 您现在应该已经对此非常熟悉。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from IPython.display import display\n",
    "import ipywidgets\n",
    "import traitlets\n",
    "from jetbot import Camera, bgr8_to_jpeg\n",
    "\n",
    "camera = Camera()\n",
    "\n",
    "image_widget = ipywidgets.Image()\n",
    "\n",
    "traitlets.dlink((camera, 'value'), (image_widget, 'value'), transform=bgr8_to_jpeg)\n",
    "\n",
    "display(image_widget)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "我们还将创建机器人实例，以驱动电机。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from jetbot import Robot\n",
    "\n",
    "robot = Robot()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "现在，我们将定义滑块来控制JetBot\n",
    ">注意：我们已经为最知名的配置初始化了滑块值，但是这些值可能不适用于您的数据集，因此请根据您的设置和环境增加或减少滑块\n",
    "\n",
    "1.速度控制（speed_gain_slider）：要启动JetBot，请增加“ speed_gain_slider”\n",
    "2.转向增益控制（steering_gain_sloder）：如果看到JetBot摇摆不定，则需要减少“ steering_gain_slider”，直到平滑为止\n",
    "3.转向偏差控制（steering_bias_slider）：如果看到JetBot偏向轨道的最右侧或最左侧，则应控制此滑块，直到JetBot开始沿中心线或轨道行进。 这说明了电机偏差以及相机偏移\n",
    "\n",
    ">注意：您应该以较低的速度绕过上述滑块，以实现平滑的JetBot道路跟随行为。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "speed_gain_slider = ipywidgets.FloatSlider(min=0.0, max=1.0, step=0.01, description='speed gain')\n",
    "steering_gain_slider = ipywidgets.FloatSlider(min=0.0, max=1.0, step=0.01, value=0.2, description='steering gain')\n",
    "steering_dgain_slider = ipywidgets.FloatSlider(min=0.0, max=0.5, step=0.001, value=0.0, description='steering kd')\n",
    "steering_bias_slider = ipywidgets.FloatSlider(min=-0.3, max=0.3, step=0.01, value=0.0, description='steering bias')\n",
    "\n",
    "display(speed_gain_slider, steering_gain_slider, steering_dgain_slider, steering_bias_slider)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "接下来，让我们显示一些滑块，这些滑块将使我们看到JetBot的想法。 x和y滑块将显示预测的x，y值。\n",
    "\n",
    "转向滑块将显示我们的估算转向值。 请记住，该值不是目标的实际角度，而仅仅是一个\n",
    "几乎成比例。 当实际角度为``0''时，它将为零，并且将随实际角度增加/减少。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x_slider = ipywidgets.FloatSlider(min=-1.0, max=1.0, description='x')\n",
    "y_slider = ipywidgets.FloatSlider(min=0, max=1.0, orientation='vertical', description='y')\n",
    "steering_slider = ipywidgets.FloatSlider(min=-1.0, max=1.0, description='steering')\n",
    "speed_slider = ipywidgets.FloatSlider(min=0, max=1.0, orientation='vertical', description='speed')\n",
    "\n",
    "display(ipywidgets.HBox([y_slider, speed_slider]))\n",
    "display(x_slider, steering_slider)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "接下来，我们将创建一个函数，只要相机的值发生变化，该函数就会被调用。 此功能将执行以下步骤\n",
    "\n",
    "1.预处理相机图像    \n",
    "2.执行神经网络      \n",
    "3.计算近似转向值      \n",
    "4.使用比例/微分控制（PD）控制电机      "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "angle = 0.0\n",
    "angle_last = 0.0\n",
    "\n",
    "def execute(change):\n",
    "    global angle, angle_last\n",
    "    image = change['new']\n",
    "    xy = model(preprocess(image)).detach().float().cpu().numpy().flatten()\n",
    "    x = xy[0]\n",
    "    y = (0.5 - xy[1]) / 2.0\n",
    "    \n",
    "    x_slider.value = x\n",
    "    y_slider.value = y\n",
    "    \n",
    "    speed_slider.value = speed_gain_slider.value\n",
    "    \n",
    "    angle = np.arctan2(x, y)\n",
    "    pid = angle * steering_gain_slider.value + (angle - angle_last) * steering_dgain_slider.value\n",
    "    angle_last = angle\n",
    "    \n",
    "    steering_slider.value = pid + steering_bias_slider.value\n",
    "    \n",
    "    robot.left_motor.value = max(min(speed_slider.value + steering_slider.value, 1.0), 0.0)\n",
    "    robot.right_motor.value = max(min(speed_slider.value - steering_slider.value, 1.0), 0.0)\n",
    "    \n",
    "execute({'new': camera.value})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "太好了！ 我们已经创建了神经网络执行功能，但是现在我们需要将其附加到相机上进行处理。\n",
    "\n",
    "我们通过观察功能来实现。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    ">警告：此代码将移动机器人！ 请确保您的机器人有间隙，并且在收集数据的Lego或Track上。 道路追随者应该可以工作，但是神经网络的性能仅取决于其训练的数据！"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "camera.observe(execute, names='value')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "太棒了！ 如果您的机器人已插入电源，则现在应该在每个新的摄像机框架上生成新命令。\n",
    "\n",
    "现在，您可以将JetBot放在收集了数据的Lego或Track上，并查看它是否可以跟随track。\n",
    "\n",
    "如果要停止此行为，可以通过执行以下代码来取消附加此回调。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "\n",
    "camera.unobserve(execute, names='value')\n",
    "\n",
    "time.sleep(0.1)  # add a small sleep to make sure frames have finished processing\n",
    "\n",
    "robot.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###  结论\n",
    "本次现场演示就是这样！ 希望您玩得开心，看到您的JetBot在轨道上顺畅地移动！！！\n",
    "\n",
    "如果您的JetBot行驶不顺利，请尝试找出失败的地方。 这样做的好处是，我们可以针对这些故障情况收集更多的数据，并且JetBot应当变得更好：)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
